
#include <stdlib.h>
#include "rclcpp/rclcpp.hpp"
#include <stdio.h>
#include <string.h>
#include <lib/md5.h>
#include <sys/stat.h>
#include <iostream>
#include <fstream>
#include "lib/base64.h"
#include "firmware/ver.h"
#include "firmware/firmware.h"
#include <boost/thread/thread.hpp>
#include <fcntl.h>      // read
#include "ftp/ftp/server.h"

namespace firmware {
const char FILEPATH[] = "/home/lgmg/agv/";
const int SERVERPORT = 4242;

Firmware::Firmware(rclcpp::Node& nh):nh_(nh)
{  
}

// get verison
bool Firmware::versionCallback(agv_srvs::srv::Version::Request &req, agv_srvs::srv::Version::Response &res)
{
    ROS_INFO_STREAM_NAMED("firmware", "ver:"<<tag<<" branch:"<<branch<<" hash:"<<hash);
    res.ver = tag;
    res.branch = branch;
    res.hash = hash;
    return true;
}

// firmware update 
bool Firmware::firmwareUpdateCallback(agv_srvs::srv::FirmwareUpdate::Request &req, agv_srvs::srv::FirmwareUpdate::Response &res)
{
    ROS_INFO_STREAM_NAMED("firmware", "fw name:"<<req.fw_name<<" size:"<<req.fw_size<<" md5:"<<req.fw_md5);
    std::string file_path = FILEPATH;

    if (req.fw_size > 0 && req.fw_content.size() > 0) {
       
        std::string content = base64_decode(req.fw_content);
        std::string fw_md5 = md5(content);
        // check firmware md5
        if (fw_md5 == req.fw_md5) {
            //Check whether the folder exists
            if (access(file_path.c_str(), 0) == -1) {          
                int flag = mkdir(file_path.c_str(), S_IRWXU);  // create new folder 
                if (flag == 0) {                               // succeed
                    ROS_INFO_STREAM_NAMED("firmware", "Create directory successfully.");
                } else {                                       // failed
                    ROS_WARN_STREAM_NAMED("firmware", "Fail to create directory.");
                    res.success = false;
                    res.message = "file path error.";
                    return -1;
                }
            }
            // wirte file to local path
            std::string file_name = file_path + req.fw_name;
            std::ofstream f(file_name.c_str(), std::ofstream::binary);
            if (!f.is_open()) {
                ROS_WARN_STREAM_NAMED("firmware", "Can't open file:"<<file_name);
                res.success = false;
                res.message = "file open error.";
                return -1;
            } else {
                f.write(reinterpret_cast<const char*>(content.data()), content.length());
                f.close();
            }
            // // decompress file to local path
            // std::string cmd = "tar -zxvf " + file_name + " -C " + file_path;
            // int ret = system(cmd.c_str());
            // if (ret != 0) {
            //     ROS_WARN_STREAM_NAMED("firmware", "Fail to decompress file.");
            //     res.success = 1;
            //     res.message = "file decompress error.";
            // }
            // // delete local tar package
            // cmd = "rm " + file_name;
            // ret = system(cmd.c_str());

            // update firmware 
            std::string fifo_path = file_path + "fwupdate";
            cmd_ = "fwupdate " + req.fw_name + "\n";
            if (access(fifo_path.c_str(), F_OK) == -1)  {
                res.success = false;
                res.message = "update error.";
                return 0;
            }
            int fifo_fd = open(fifo_path.c_str(), O_WRONLY);
            if (fifo_fd < 0) {
                ROS_WARN_STREAM_NAMED("firmware", "Fail to open fifo file.");
                res.success = false;
                res.message = "fifo error.";
            } else {
                int ret = write(fifo_fd, cmd_.data(), cmd_.size()); 
                if (ret <= 0) {
                    ROS_WARN_STREAM_NAMED("firmware", "Fail to write fifo file.");
                    res.success = false;
                    res.message = "fifo write error.";
                } else {
                    ROS_INFO_STREAM_NAMED("firmware", "update firmware:"<<cmd_);
                    res.success = true;
                    res.message = "successed.";
                }
                close(fifo_fd);
            }
        } else {
            ROS_WARN_STREAM_NAMED("firmware", "Fail to decompress file.");
            res.success = false;
            res.message = "file md5 error.";
        }
    } else {
        ROS_WARN_STREAM_NAMED("firmware", "Fail to decompress file.");
        res.success = false;
        res.message = "file size error.";
    }   
    res.success = true;
    res.message = "successed.";
    return true;
}

void Firmware::spin()
{
    ROS_INFO_STREAM_NAMED("firmware", "start firmware thread");
    ioService.run();
}

void Firmware::init()
{
    update_ = nh_.advertiseService("/fw_update", &Firmware::firmwareUpdateCallback, this);
    ver_ = nh_.advertiseService("/fw_version", &Firmware::versionCallback, this);

    try {
        server = new Server(ioService, SERVERPORT, FILEPATH);
        work.reset(new boost::asio::io_service::work(ioService));

    } catch (std::exception& e) {
        std::cerr << "Exception: " << e.what() << "\n";
    }
}

}  // end namespace

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto nh = rclcpp::Node::make_shared("firmware");
    setlocale(LC_ALL, "");

    firmware::Firmware fw(nh);
    fw.init();
    
    boost::thread t = boost::thread(boost::bind(&firmware::Firmware::spin, &fw));

    rclcpp::spin(node);
    
    fw.stop();
    t.interrupt();
    t.join();
    
    return 0;
}
